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High-Order  Averaging  on  Lie  Groups  and  Control  of  an 
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Naomi  Ehrich  Leonard2  and  P.  S.  Krishnaprasad 
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Department  of  Electrical  Engineering 
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Abstract 

In  this  paper  we  extend  our  earlier  results  on  the  use  of  periodic  forcing  and  averaging 
to  solve  the  constructive  controllability  problem  for  drift-free  left-invariant  systems  on 
Lie  groups  with  fewer  controls  than  state  variables.  In  particular,  we  prove  ,a  third- 
order  averaging  theorem  applicable  to  systems  evolving  on  general  matrix  Lie  groups 
and  show  how  to  use  the  resulting  approximations  to  construct  open  loop  controls  for 
complete  controllability  of  systems  that  require  up  to  depth-two  Lie  brackets  to  sat¬ 
isfy  the  Lie  algebra  controllability  rank  condition.  The  motion  control  problem  for  an 
autonomous  underwater  vehicle  is  modelled  as  a  drift-free  left-invariant  system  on  the 
matrix  Lie  group  SE(3).  In  the  general  case,  when  only  one  translational  and  two  an¬ 
gular  control  inputs  are  available,  this  system  satisfies  the  controllability  rank  condition 
using  depth-two  Lie  brackets.  We  use  the  third-order  averaging  result  and  its  geomet¬ 
ric  interpretation  to  construct  open  loop  controls  to  arbitrarily  translate  and  orient  an 
autonomous  underwater  vehicle. 

1This  research  was  supported  in  part  by  the  National  Science  Foundation’s  Engineering  Research 
Centers  Program:  NSFD  CDR  8803012,  by  the  AFOSR  University  Research  Initiative  Program  under 
grant  AFOSR-90-0105,  and  by  the  Army  Research  Office  under  Smart  Structures  URI  Contract  No. 
DA  AL03-92-G-0 12 1 . 

Supported  in  part  by  the  Zonta  International  Foundation. 
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1  Introduction 


Drift-free  systems  with  fewer  controls  than  state  variables  arise  in  a  variety  of  control 
problems  including  motion  planning  for  wheeled  robots  subject  to  nonholonomic  con¬ 
straints,  spacecraft  attitude  control  and  the  motion  control  of  autonomous  underwater 
vehicles.  The  basic  state-space  model  takes  the  form 

m 

x  =  Fj(x)uj,  X  e  9£n,  u  =  (ui, . . .  ,um)  6  n>m.  (1) 

i=i 

It  is  well  known  that  if  the  vector  fields  Fi  satisfy  a  Lie  algebra  rank  condition,  then 
there  exists  a  control  u  that  drives  the  system  to  the  origin  from  any  initial  state.  How¬ 
ever,  unlike  the  linear  setting  where  the  controllability  Grammian  yields  constructive 
controls,  here  the  rank  condition  does  not  lead  immediately  to  an  explicit  procedure  for 
constructing  controls.  As  a  result,  recent  research  has  focused  on  constructing  controls 
to  achieve  complete  controllability  [1,  2,  3,  4,  5,  6].  In  particular,  constructive  procedures 
based  on  periodically  time- varying  controls  have  proven  successful  [3,  4,  5,  6]. 

Our  interest  in  this  paper  is  in  constructive  controllability  using  periodic  forcing  of 
drift-free  left-invariant  systems  of  the  form 

X  =  zXU,  U(t)  =  itAiui(t),  (2) 

i=i 

evolving  on  matrix  Lie  groups.  Here  A”(f)  is  a  curve  in  a  matrix  Lie  group  G  of  dimension 
n,  U(t)  is  a  curve  in  the  Lie  algebra  Q  of  G,  and  {Ai, . . . ,  An}  a  basis  for  Q.  (For  an 
introduction  to  matrix  Lie  groups  and  Lie  algebras  see  [7]).  The  «,■(•)  are  assumed  to  be 
periodic  functions  of  common  period  T.  t  >  0  is  a  small  parameter  such  that  eu.-(-)  are 
interpreted  as  the  small-amplitude  periodic  controls,  although  some  of  the  «<(•)  may  be 
identically  zero. 

Equation  (2)  describes  the  kinematics  of  several  types  of  important  systems.  For  ex¬ 
ample,  equation  (2)  describes  rigid  spacecraft  kinematics  if  we  interpret  U ( t )  as  the  time- 
dependent  skew  symmetric  matrix  of  spacecraft  angular  velocity  such  that  X  evolves  on 
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G  =  50(3),  the  special  orthogonal  group,  where 


SO(n)  =  {Ae  &nxn| AtA  =  I ,  det(A)  =  1}. 


Similarly,  equation  (2)  describes  the  kinematics  of  an  underwater  vehicle  if  we  interpret 
U ( t )  as  the  time-dependent  matrix  of  vehicle  angular  and  translational  velocities  such 
that  X  evolves  on  G  —  SE( 3),  the  special  Euclidean  group,  where 


SE{n)  =  { 


A  b 
0  1 


€  3J("+I)x(-+1)[ A  e  S0(n),be  £n}. 


We  state  formally  the  complete  constructive  controllability  problem  for  system  (2) 
where  =  0,  i  =  m  +  1, . . . ,  n: 


(P)  Given  an  initial  condition  X{  G  G,  a  final  condition  Xj  G  G  and  a  time  tj  >  0,  find 
u(t)  =  t  €  [0,t/],  such  that  X(0)  =  X{  and  X(tf)  =  X/. 

Our  objective  is  to  prescribe  means  to  solve  (P)  using  small-amplitude  periodic  controls. 
One  strategy  is  to  use  periodic  controls  to  provide  open  loop  control  of  the  system  and 
apply  intermittent  feedback  corrections  to  make  finer  adjustments  in  system  behavior. 
This  strategy  allows  us  to  take  advantage  of  a  priori  knowledge  of  the  system  and 
prescribe  efficient  open  loop  controls  to  drive  the  system  as  desired  without  sacrificing 
accuracy  and  sensitivity  reduction  associated  with  feedback  control.  We  use  averaging 
theory  for  systems  of  the  form  (2)  as  a  means  to  specify  open  loop  periodic  control. 
The  goal  of  averaging  in  this  context  is  to  describe  an  approximate  solution  to  (2)  that 
evolves  on  the  matrix  group  G  and  remains  close  to  the  actual  solution,  but  gives  rise 
to  straightforward  procedures  for  achieving  complete  constructive  controllability. 

First  and  second-order  averaging  theorems  have  been  proved  for  systems  of  the  form 
(2)  [8,  6].  The  first-order  averaging  results  reveal  the  effect  of  the  dc  component  of 
the  periodic  forcing  on  the  behavior  of  the  system,  but  are  only  useful  for  complete 
constructive  controllability  if  none  of  the  controls  «,•(•)  is  identically  zero.  However,  the 
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second-order  average  approximation  provides  a  formula  for  achieving  complete  construc¬ 
tive  controllability  using  fewer  than  n  periodic  controls  if  the  controllability  Lie  algebra 
rank  condition  is  satisfied  for  a  system  of  the  form  (2)  using  up  to  depth-one  Lie  brack¬ 
ets  (i.e.,  single  brackets).  In  this  case  the  formula  solves  (P)  with  0(e2)  accuracy  which 
could  be  improved  with  intermittent  feedback  if  desired.  Additionally,  the  second-order 
average  approximation  admits  a  geometric  interpretation  as  an  area  rule.  This  was  used 
to  advantage  in  the  design  of  open  loop  controls  for  the  spacecraft  attitude  control  prob¬ 
lem  with  only  two  controls  available  and  for  the  unicycle  motion  planning  problem  (c.f. 
[6])- 

In  this  paper  we  prove  a  third-order  averaging  theorem  for  systems  of  the  form  (2) 
and  develop  the  associated  geometric  interpretation.  This  facilitates  the  design  of  open 
loop  controls  to  solve  (P)  with  0(e3)  accuracy  for  systems  (2)  which  require  up  to 
depth-two  Lie  brackets  (i.e.,  double  brackets)  to  satisfy  the  controllability  Lie  algebra 
rank  condition.  We  apply  this  result  to  the  problem  of  specifying  controls  to  drive  an 
autonomous  underwater  vehicle  to  a  desired  position  and  orientation  when  only  three 
controls  are  available  (two  rotational  and  one  translational). 

Autonomous  underwater  vehicles  can  potentially  be  sent  into  environments  too  risky 
for  a  manned  vehicle  and  too  deep  for  a  tethered  vehicle.  Thus,  they  are  expected  to 
play  an  increasingly  larger  role  in  oceanic  exploration  and  exploitation,  for  example, 
in  geological  surveying,  data  collection,  drill  support,  construction,  maintenance,  etc. 
[9].  Similarly,  there  is  great  potential  for  their  use  in  other  types  of  hazardous  environ¬ 
ments  such  as  in  nuclear  reactor  vessels,  e.g.,  for  inspection  and  maintenance.  Further, 
with  the  advent  of  micro-machining  and  micro-technologies  comes  the  prospect  of  using 
micro-scale  autonomous  underwater  vehicles  for  micro-scale  underwater  tasks  such  as 
in  medical  applications,  for  example,  to  send  through  blood  vessels  or  arteries  for  organ 
inspection  or  repair. 

To  achieve  autonomy  of  the  underwater  vehicle  in  each  of  these  settings,  the  nonlinear 
behavior  of  the  vehicle  must  be  controlled.  As  described  above,  system  (2)  models 
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the  motion  control  problem  if  we  can  interpret  the  vehicle  angular  and  translational 
velocities  as  our  control  inputs.  This  interpretation  means  that  we  assume  that  we 
can  independently  actuate  these  velocities  (or  at  least  some  of  them)  as  desired.  For 
example,  by  controlling  a  propeller  at  the  back  of  the  vehicle,  stern  and  bow  planes  on 
the  sides  of  the  vehicle  and  rudder  planes  at  the  back  of  the  vehicle,  the  three  angular 
velocities  and  one  translational  velocity  can  be  controlled  [10]. 

In  the  special  case  of  a  micro-scale  underwater  vehicle  or  a  relatively  small  vehicle 
in  a  highly  viscous  fluid,  angular  and  translational  velocities  can  be  effected  simply 
by  cyclic  body  deformations.  This  special  case  is  the  case  of  low  Reynolds  number 
(Re  <  1)  in  which  frictional  forces  between  the  vehicle  and  the  fluid  dominate  while 
inertial  forces  are  negligible.  Motion  in  this  context  has  been  studied  by  physicists 
interested  in  understanding  how  microorganisms  such  as  paramecia  swim  [11,  12,  13,  14]. 
In  imitation  of  the  flagella  or  cilia  used  by  microorganisms  for  maneuvering,  actuators 
such  as  flapping  flexible  oars  or  rotating  corkscrews  could  be  used  to  generate  cingular 
and  translational  velocities  for  the  vehicle  at  low  Reynolds  number. 

In  [10]  a  globally  stable  nonlinear  tracking  controller  was  developed  using  three 
angular  velocities  and  one  translational  velocity  as  control  inputs.  For  this  controller, 
the  kinematic  equation  was  given  by  Euler  angles  and  the  reference  trajectory  was 
assumed  to  have  a  non-zero  velocity.  In  [15]  an  exponentially  convergent  stabilizing 
control  law  was  presented  using  again  three  angular  velocities  and  one  translational 
velocity  as  control  inputs.  In  this  case  the  kinematics  were  modelled  as  in  (2).  In  this 
paper  we  need  only  require  authority  over  two  angular  velocities  and  one  translational 
velocity  to  translate  and  orient  an  underwater  vehicle  as  desired.  The  low  number  of 
controls  required  to  achieve  complete  constructive  controllability  provides  a  measure 
of  redundancy  to  the  control  system.  This  redundancy  can  also  be  interpreted  as  the 
means  for  the  controller  to  “adapt”  to  a  failure  in  the  system  that  reduces  the  control 
authority,  by  continuing  to  provide  complete  control  over  the  position  and  orientation 
of  the  vehicle. 
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In  Section  2  we  summarize  the  results  from  our  work  on  first-order  and  second-order 
averaging  [8,  6,  16].  In  Section  3  we  discuss  the  basic  ideas  behind  high-order  averaging 
and  prove  a  third-order  averaging  theorem  for  general  matrix  Lie  groups.  Our  main 
result  is  an  “area-moment  rule”  (Theorem  5)  for  systems  on  groups.  We  discuss  the 
geometric  interpretation  of  this  rule  and  the  consequences  for  control  illustrating  how 
to  use  the  averaging  result  to  achieve  complete  constructive  controllability  for  systems 
which  meet  the  controllability  rank  condition  with  up  to  depth-two  Lie  brackets.  In  Sec¬ 
tion  4  we  examine  our  main  result  for  the  Lie  group  SE( 3)  in  the  context  of  underwater 
vehicle  control.  An  example  is  given  for  achieving  complete  constructive  controllability 
when  only  one  translational  and  two  angular  controls  are  available. 


2  First  and  Second- Order  Averaging 

Since  there  are  no  explicit  global  representations  of  the  solution  to  (2)  we  make  use 
of  local  representations:  the  product  of  exponentials  representation  given  by  Wei  and 
Norman  [17]  and  the  single  exponential  representation  given  by  Magnus  [18].  The  basic 
idea  which  we  use  for  first  and  second-order  averaging  as  well  as  for  high-order  averaging 
is  to  derive  classical  averaging  theory  approximations  for  the  local  representation  and 
then  transfer  such  estimates  to  the  group  level  for  solutions  to  (2).  We  begin  by  defining 
the  Wei-Norman  representation  and  summarizing  the  associated  averaging  results. 

Lemma  1  (Wei  and  Norman).  Let  X(t)  be  the  solution  to  (2)  and  X(0)  =  I.  Then 
3 10  >  0  such  that  for  \t\  <  t0 ,  X(t )  can  be  expressed  in  the  form 

X(t)  =  esWMie32(«M2 . . .  e9n{i)An  _  (3) 

The  Wei-Norman  parameters  g  =  (gx, . . . ,  gn)T  satisfy 

g  =  tM(g)u,  for  |f|  <  f0  ,  (4) 

where  g(0)  =  0  and  M(g)  is  a  real  analytic  matrix-valued  function  of  g.  If  Q  is  solvable 
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then  there  exists  a  basis  of  Q  and  an  ordering  of  this  basis  for  which  (4)  holds  globally, 
i.e.,  for  all  t.  □ 

It  is  customary  to  refer  to  components  of  g  as  the  canonical  coordinates  of  the  second 
kind  for  G.  Let  W  be  the  open  neighborhood  of  0  €  such  that  'ig  6  W,  M(g )  is 
well-defined.  Let  $  :  9in  —>  G  define  the  mapping 

$(g)  =  e9lAle92A2  •  •  •  e3"'4"  (5) 

and  define  V  =  $(kL)  C  G.  Then,  the  Wei-Norman  formulation  provides  a  local 
representation  of  the  solution  to  (2)  for  initial  condition  X(0)  €  V  C  G.  Now  let  S 
be  the  largest  neighborhood  of  0  €  contained  in  W  such  that  4/  =  $|s  :  S  — *  G 
is  one-to-one.  Let  Q  =  'P(.S')  C  V.  Then  :  S  — *  Q  is  a  diffeomorphism  and  we  can 
define  a  metric  d  :  Q  x  Q  —i ►  5?+  by 

d(Y,Z)  =  d(<t-l(Y),t-\Z))  (6) 

where  d  :  x  3?"  — >  !R+  is  given  by 

d{a,  P)  =  ||<*  “  Ph  =  Z  Ia»'  “  Pi 

i- 1 

The  averaged  system  associated  with  (4)  is  defined  as 

g  =  eM(g)(^  ^  u(r)dr) 

=  eM(g)uav ,  5(0)  =  g0  (8) 

where  we  assume  that  M(go)  is  well-defined.  The  average  solution  X(t)  associated  with 
the  solution  X(t )  of  (2)  is  defined  as 

.  n 

X  =  eXUav,  Uav  —  Z1  AjUavj  (9) 

i=l 

where  uav  =  (uavl, . . . ,  uavn)T.  Thus, 

X{t)  =  X(0)eeU^.  (10) 
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The  first-order  averaging  theorem  can  now  be  stated  (c.f.  Theorem  2  of  [8]). 

Theorem  1.  Let  e  >  0  be  a  small  parameter.  Let  D  =  {g  6  3£n  1  \\g\\  <  r}  C  S. 
Assume  that  u(t)  £  is  periodic  in  t  with  period  T  >  0  and  has  continuous  derivatives 
up  to  second  order  for  t  €  [0,oo).  Let  X(t)  be  the  solution  to  (2)  represented  by  (3) 
where  g(t,e)  is  the  solution  to  (4)  with  ty(sr(0,e))  =  X(0)  and  g( 0,  e)  €  D.  Let  X(t )  be 
the  solution  to  (9)  and  let  g(t,  e)  be  the  solution  to  (8)  with  $(<7(0,  e))  =  X(0). 

If  g(t ,  e)  €  D,  Vi  €  [0,  6/e]  and  ||flf(0,  e)  -  <j(0,  e)||  =  0(e) 

then  d(X(t),X(t))  =  0(e),  Vi  €  [0,6/e].  □ 

Next  we  show  the  form  of  the  second-order  average  approximation  X(t)  to  the  so¬ 
lution  X(t)  of  (2).  Let  g  =  (<7X, . . .  ,gn)T  be  the  second-order  average  approximation  of 
the  solution  g(t)  to  (4).  We  define  the  second-order  approximation  X  on  the  group  level 
as 

1(7)  =  eUt)MeUt)A,  . . .  e5„(<Mn)  (H) 

which  is  well-defined  for  g(t)  well-defined.  To  isolate  the  second-order  effect  we  assume 
that  uav  =  0.  We  define  u  —  (ui, . . . ,  un)T  by 

ut(i)  =  /  Ui(r)d,T.  (12) 

Jo 

So  u  ~  u  and  u  is  periodic  in  t  with  common  period  T.  Let  U  —  ^"=1  Next  we 

define  Area,y(T)  to  be  the  area  bounded  by  the  closed  curve  described  by  ft,-  and  uj  over 
one  period,  i.e.,  from  t  =  0  to  t  =  T.  By  Green’s  Theorem  we  can  express  this  area  as 

1  [T 

Areaij{T)  =  —  /  (u,(cr)uj(cr)  —  Uj{cr)ui{a))d(T.  (13) 

2  Jo 

This  area  can  be  interpreted  as  the  projection  onto  the  i-j  plane  of  the  area  enclosed 
by  the  curve  (£ti,...,un)  in  one  period.  Finally,  we  define  the  structure  constants  F^ 
associated  with  the  basis  {Ai, . . . ,  An}  for  the  Lie  algebra  Q  of  G  by 

=  ij  =  i,...,-  (H) 

fc= 1 
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where  [•,  •]  is  the  Lie  bracket  on  Q  defined  by  [A,  B ]  =  AB  —  BA. 

The  second-order  averaging  theorem  can  now  be  stated  (c.f.  Theorem  2  of  [6]). 

Theorem  2  (Area  Rule).  Let  e  >  0  be  a  small  parameter.  Let  D  =  {g  £  \  ||<7||  < 

r}  C  S.  Assume  that  u(t)  G  is  periodic  in  t  with  period  T  >  0  and  has  continuous 
derivatives  up  to  third  order  for  t  G  [0,co).  Suppose  that  uav  =  0.  Let  X(t)  be  the 
solution  to  (2)  represented  by  (3)  where  g(t ,  c)  is  the  solution  to  (4)  with  g( 0,  e)  =  go  G  D 
such  that  $(</o)  =  -X’(O)  and  ||<7o||  =  0(e).  Define 

**(*»  e)  =  £  Area.-^T)^,  k  =  1, . . . ,  n, 

g  =  z  +  eu  +  %, 

X(t)  =  eplWM  e92(t)A2  .  .  . 

where  ||<70  -  loll  =  °(e2).  If  ( z(t ,  e)  +  g0)  G  D,  Vt  €  [0,  6/e]  then 

d(X(i),l(t))  =  0(e2),  Vi  €  [0,6/e].  □ 

We  can  state  analogous  first  and  second-order  averaging  results  based  on  the  single 
exponential  local  representation  of  solutions  to  (2).  By  Theorem  III  of  [18],  assuming  a 
certain  convergence  criterion  is  met,  the  solution  to  (2)  with  X(0)  =  I  can  be  expressed 
as 

X(t)  =  em  (18) 

where  Z(t)  G  Q  is  given  by  the  infinite  series  (we  show  terms  up  to  0(e3)): 

Z(t)  =  t  f  U(T)dT  +  ~  f  [U(t),U(t)\<It 
Jo  Z  J 0 

+  j  j‘{JjU(o),U(a)]da,  V(t )\dr  +  ~  Jju(r),  [U( r),  U(r)\\dr  +  ...  .(19) 

Satisfying  the  convergence  criterion  means  limiting  the  duration  of  validity  of  the  single 
exponential  representation  (see  [16]  for  details).  Assuming  the  convergence  requirement 
is  met,  then  Z(t)  is  the  solution  to 

Z  =  eU+j{U,U]  +  j{J\u(T),U(T)]dT,U]  +  ^{U,{U,U}}  +  ---,  Z(0)=0.  (20) 


(15) 

(16) 
(17) 
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Let  4  :  Q  — ►  G  define  the  mapping 

$(Z)  =  ez.  (21) 

Let  S  be  the  largest  neighborhood  of  0  €  Q  such  that  4*  =  $1^  :  S  — >  G  is  one-to-one. 
Let  Q  =  4(5)  C  G.  Then  4'  :  S  — »  Q  is  a  diffeomorphism  and  we  can  define  a  metric 
d  :  Q  x  Q  — *  !R+  by 

d(x,F)  =  d(4-1(X),4-1(y))  (22) 

where  d  is  given  by  (6). 

The  second-order  averaging  theorem  based  on  the  single  exponential  representation 
is  given  below  (c.f.  Theorem  4  [16]).  The  first-order  averaging  theorem  using  this 
representation  gives  the  same  result  as  Theorem  1. 

Theorem  3  (Single  Exponential  Area  Rule).  Let  e  >  0  be  a  small  parameter.  Let 
D  =  {Z  e  Q\\\Z\\  <  r}  C  S.  Assume  that  u(t)  G  is  periodic  in  t  with  period  T  >  0 
and  has  continuous  derivatives  up  to  third  order  for  t  G  [0,oo).  Suppose  uav  0.  Let 
b  >  0  be  such  that  the  convergence  requirement  for  (19)  is  met  Vt  G  [0,b/e\.  Let  X(t) 
be  the  solution  to  (2),  with  X(0)  =  /,  represented  by  the  single  exponential  (IS)  where 
Z(t,e)  G  Q  is  the  solution  to  (20).  Define 


2(<>t)  =  yE(  S  Areai3(T)T^)Ak, 

(23) 

Xs(t)  =  eM. 

(24) 

If  Z(t,e)  G  A  Vt  G  [0,  &/e]  then 

d(X(t),Xs(t))  =  0(e2)  on  [0,6/e].  □ 

It  is  clear  from  equation  (9)  that  the  first-order  approximation  X(t )  describes  the 
effect  of  the  dc  component  of  the  control  input  u  on  the  system  (2).  Thus,  the  approx¬ 
imation  provides  a  formula  for  complete  constructive  controllability  only  if  none  of  the 
controls  ut(-)  is  identically  zero.  On  the  other  hand,  the  second-order  average  approxi¬ 
mation  provides  a  formula  for  complete  controllability  even  when  some  of  the  controls 
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are  identically  zero.  Specifically,  the  second-order  average  approximation  X(t)  given  by 
equations  (15)  -  (17)  (or  similarly  Xs  given  by  (23)  and  (24)),  in  providing  more  infor¬ 
mation  about  the  actual  solution  to  (2),  captures  the  effect  of  the  group  level  version 
of  depth-one  Lie  brackets.  This  effect  is  stated  in  the  next  theorem  (c.f.  Theorem  3  of 
[6])- 

Theorem  4.  Suppose  that  system  (2)  satisfies  the  Lie  algebra  controllability  rank  con¬ 
dition  with  up  to  depth-one  Lie  brackets.  Then  the  complete  constructive  controllability 
problem  (P)  can  be  solved  with  0(e2)  accuracy  using  the  formula  for  X(t)  given  by  (15) 
-  (17)  or  the  formula  for  %s(t)  given  by  (23)  and  (24).  □ 

Basically,  this  theorem  tells  us  that  in  the  formula  (17)  for  X  (and  analogously  for 
Xs),  each  gk  will  be  the  linear  combination  of  terms  like  uk  and  terms  like  Area{j(T ) 
and  no  gk  will  be  identically  zero  (or  constant).  It  is  then  easy  to  see  how  to  construct 
open  loop  controls  since  we  know  the  geometric  meaning  of  the  terms  Area,ij(T),  i.e., 
that  Area,ij(T)  is  the  area  bounded  by  the  closed  curve  described  by  u,-  and  iij  over 
one  period.  In  particular,  if  we  choose  and  Uj  to  be  sinusoids  that  are  in  phase  then 
Area,j(T)  =  0.  Alternatively,  if  they  are  chosen  out  of  phase  then  AreaX](T)  0  can 
be  computed  based  on  the  signal  magnitudes  and  their  phase  difference.  An  algorithm 
can  then  be  derived  based  on  this  geometric  reasoning. 

The  spacecraft  attitude  control  problem,  where  it  is  assumed  that  two  angular  veloc¬ 
ities  are  available  as  control  inputs  (e.g.,  in  the  case  where  there  are  two  reaction  wheels 
and  no  external  torque  is  applied),  can  be  modelled  as  (2)  with  n  =  3  and  u3(-)  =  0. 
It  can  be  shown  that  this  system  satisfies  the  hypothesis  of  Theorem  4  and  with  the 
appropriate  choice  of  basis  elements  {A\,Ai,  A3},  X  takes  the  form 

X(t)  =  e^AlehA2ehA3 

_  e(e«l+5oiMl  e(««2+ffo2)^2  e(^^ea12{T)+g03)A3  (25) 

Similarly,  the  unicycle  motion  planning  problem  can  be  modelled  by  (2)  with  G  =  SE( 2), 
n  —  3  and  u3(-)  =  0.  This  system  also  satisfies  the  hypothesis  of  Theorem  4  and  X 
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again  takes  the  form  (25).  Details  on  an  algorithm  that  was  derived  for  these  systems 
as  well  as  corresponding  simulation  results  can  be  found  in  [6]. 


3  Third- Order  Averaging 

Higher-order  average  approximations  to  the  solution  to  (2)  naturally  provide  successively 
more  information  about  the  actual  solution,  X{t).  The  nature  of  this  information  can  be 
gleaned  from  the  infinite  series  expansion  of  Z(t)  in  (19),  where  it  is  noted  that  by  (18) 
Z(t)  is  the  logarithm  of  X(t).  The  0(e2)  term  in  (19)  is  a  depth-one  Lie  bracket  and 
as  verified  in  Theorem  4  of  the  previous  section  the  0(e2)  approximation  completely 
captures  the  effect  of  the  depth-one  Lie  brackets  in  the  context  of  controllability.  It 
is  expected  that  the  0(ep)  approximation  for  p  >  2  of  X(t)  will  completely  capture 
the  effect  of  depth- (p  —  1)  Lie  brackets  in  the  context  of  controllability.  (Note  that 
a  depth-(p  —  1)  Lie  bracket  is  defined  as  (p  —  1)  iterated  brackets,  e.g.,  a  depth-two 
bracket  is  of  the  form  [A,  [B,C]],  a  depth-three  bracket  is  of  the  form  [A,  [B,  [(7,  D]]], 
A,  B,C,D  (E  Q,  etc.)  In  this  section  we  prove  this  result  for  p  =  3.  Additionally, 
we  show  that  the  third-order  approximation  has  a  geometric  interpretation  based  on 
a  higher-order  geometric  object  which  can  be  described  as  a  first  moment.  The  first 
moment  plays  a  role  analogous  to  the  role  played  by  area  in  the  second-order  average 
approximation. 

As  in  the  case  of  second-order  averaging  in  Theorem  3,  we  use  the  single  exponential 
representation  (18)  of  solutions  to  (2)  with  the  associated  differential  equation  for  Z{t) 
given  by  (20)  as  a  means  to  do  third-order  averaging.  As  in  the  case  of  second-order 
averaging  we  assume  that  uav  =  0.  Let  u  be  as  defined  by  (12).  Define 

aij(t)  =  \  j  ( Ui(a)uj(a )  -  Uj(a)ui(a))da.  (26) 

L  Jo 

Then  a.ij{t)  is  of  the  form 

«>,«)  -  + m 
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where  f(t  +  T)  =  f(t),  /( 0)  =  0,  T  is  the  period  of  the  control  u  and  Area^iT)  is  given 
by  (13).  So,  in  particular, 

a*j{qT)  =  qArea.ij(T)  (27) 


where  q  is  a  positive  integer.  Let 

1  rT 

mijk(,T)  =  -  J  -ui(cr){t,(cr)){tA:(cr)dcr.  (28) 

Now  consider  the  closed  curve  C  defined  by  Uj(t )  and  Uk(t)  over  one  period,  i.e., 

from  t  =  0  to  t  =  T.  From  (28)  we  get  that 


mijk{T )  =  —  UiUkdHj  —  UjUkdlLi . 
O  J  c 


(29) 


Let  A  be  any  oriented  surface  with  boundary  dA  =  C.  Then  by  Stokes’  Theorem, 

Triijk(T)  =  —  f  —UidujdUk  —  UjdukdiLi  +  2ujtdu,duj.  (30) 

o  Ja 

So  rriijk(T )  as  described  by  (30)  can  be  interpreted  as  the  first  moment,  i.e.,  a  linear 
combination  of  u,-  integrated  over  the  area  of  the  projection  of  A  onto  the  j-k  plane,  re¬ 
integrated  over  the  area  of  the  projection  of  A  onto  the  k-i  plane  and  Uk  integrated  over 
the  area  of  the  projection  of  A  onto  the  i-j  plane. 


Next  we  define  depth-two  structure  constants  associated  with  basis  {Ai, . . . ,  An} 
for  the  Lie  algebra  Q  in  terms  of  the  structure  constants  T-)  defined  by  (14)  as 

^‘£4  rft.  (31) 

i=i 

This  definition  comes  from  the  computation  of  structure  constants  for  depth-two  Lie 
brackets  as  follows: 

[[A.-.AjUj  =  =  ErtKA*]  =  EE^rfA  =  E  (32) 

l=\  /=!  p=l  /=!  p=l 


Theorem  5  (Area- Moment  Rule).  Let  e  >  0  be  a  small  parameter.  Let  D  =  {Z  € 
1 1|  Z ||  <  r)  C  S.  Assume  that  u(t)  €  is  periodic  in  t  with  period  T  >  0  and  has 

continuous  derivatives  up  to  fourth  order  for  t  6  [0, 00).  Let  b  >  0  be  such  that  the 
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convergence  requirement  for  (19)  is  met  Vi  G  [0 ,  6/e].  Let  X(t)  be  the  solution  to  (2), 
with  X(0)  =  I,  represented  by  the  single  exponential  (18)  where  Z(t,e)  G  Q  is  the 
solution  to  (20).  Define 

=  +  t,  “iiOK-yt  t  rmrtTWjJA,,  (33) 

p= 1  k— 1  i,j—l-,i<j 

x^\t)  =  ez(3)M.  (34) 


If  Z^(t,  e)  €  D,  Vi  €  [0, 6/e], 

d(X(t),X(3\t))  =  0(e3),  Vi  €  [0,6/e]. 


Proof.  Let  s  =  ei.  Then  from  (20), 

^  =  P  +  |[M  +  j[jf'[t/(r),^(r)]*,t/]  +  ^[i(,[&,a]]  +  ... 

=  /(*,<)•  (35> 

Let  Z0(s,e),  Zi(s,e)  and  Z2(s,e)  be  the  solutions,  respectively,  to 

^£=/(»,0)  =  C7(s),  Zo(0,e)  =  0,  (36) 

as 

^  =  21(5,0)  =  i[C/,(7]W,  Z,(0,e)=0,  (37) 

~  =  ^(»,  0)  =  h  [’m r),  (7(r))dr,  £/«]  +  i[&,  [0,  2,(0,  t)  =  0.  (38) 

as  de“  4  Jo 

Then  by  standard  perturbation  theory  (c.f.  Theorem  7.1  [19]),  if  Z0(s ,  e)  G  D,  Vs  G  [0,  6], 
then  3e*  >  0  such  that  V|e|  <  e*  (35)  has  the  unique  solution  Z(s,e)  defined  on  [0,6] 
such  that 

|| Z(s,  e)  —  ( Zo(s ,  e)  +  e^i(>s,  e)  +  e2Z2(s ,  e))||  =  0(e  ),  Vs  G  [0,  6]. 

This  implies  that 

\\Z(t,  e)  —  (Z0(i,  e)  +  eZi(t,  e)  +  e2Z2(t,  e))||  =  0(e3),  Vi  G  [0,6/e],  (39) 

where  by  (36)  -  (38)  and  since  ds  =  edi, 

Z0  =  eU(t),  Zo(0,e)  =  0,  (40) 
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Zi  =  \[U,m),  Z.(0,e)  =  0, 


=  |[ j\u( r),  C(r)]dT,  [/(<)]  +  [&,  t/]](<),  3.(0,  <0  =  0. 


Now  let  Y  =  £2Z2.  Then 


||Z  -  (Z0  +  eZi  +  V)||  =  0(e3),  Vi  €  [0, 6/e]  and 
e3.e<-  -3 


(41) 

(42) 

(43) 

(44) 


Y  =  j{  Jju(r),U(r))dT,  U(t)]  +  ^[U,  [U,  £/]]((),  Y(0,  e)  =  0. 

Let  Y(t,e)  be  the  solution  to 

?  =  ~  [\[[U(<T),U(a)\d<,,  U(r)\dr  +  A.  [&(t),  U(t)}}<1t,  Y( 0,  e)  =  0. 

(45) 

By  classical  averaging  (c.f.  Theorem  7.4  [19]),  if  Y  G  D,  Vt  €  [0,  &/ e]  and  e  is  small 
enough  then  \\Y(t,  e)-Y(t ,  e)||  =  0(e3)  on  [0,  6/e].  So  by  (43)  and  the  triangle  inequality 


\\Z(t,6)-ZW(t,e)\\  =  0(e3),  Vt  €[0,6/*], 


(46) 


where 

Z^3\t,  e)  =  Zo(t,  e)  +  zZ\(t,  e)  +  Y(t,  e) 

=  eU{t)  +  ^- Jo[U,U}{T)dT+^;  JQ  [Jo[U(a),U{a)]dcr,U{T)}dT 

+  J0  [U{r)AU(T),U{T)]]dT,  (47) 

by  (40),  (41)  and  (45).  Thus,  by  (46)  and  the  definition  of  d,  d(X(f),X(3)(t))  =  0(e3), 
Wt  €  [0,  6/e],  where  X ^  is  defined  by  (34). 

The  proof  is  complete  if  we  show  that  Z^  defined  by  (47)  agrees  with  (33).  By 
definition,  we  have  that  eU(t )  =  X)p=i  £up(i)AP.  Next  we  show  that  the  second  term  on 
the  right  side  of  (47)  is  equivalent  to  the  second  term  on  the  right  side  of  (33). 

~  [  [U ,  U](cr)d<T  =  %-  /  E«i(ff)4E«j(^;]<io' 

1  Jo  l  Jo  j=i 

e2  ft  n  L 

=  —  /  'y  ]  ( UiUj  {t,Uj)(<7)[zli,  Aj]dcr 

2  i,j=l;i<j 

=  X>2  E  «u(0rr,)Ap,  (48) 
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where  we  have  used  the  fact  that  [A^Aj]  =  -[A,,  A]  and  the  definitions  of  and 

r?.. 

V 

Next  we  examine  the  third  term  on  the  right  side  of  (47). 

2 .  ^ 

Ifi 

=  /  [/  IE  52  52  it(T)/l>J</T 

J0  *=l  j=l  k=i 

=  J^E  E  (/  (/  (“i«i  -fi,fij)(<T)d<7)ufc(r)dr)[[A,-,AJ],Afc] 

^  *=1  -70  1/0 

=  "Et^E  E  \  [  (HT)Uj(T)  -  Uj(T)ilt(T))uk(T)dT6?jkAp 

P= 1  ^  J  fc=l  i,j=l;i<j  0  J0 

=  -E(j$E  E  (49) 

p=l  fc=l 

where  we  have  used  (32),  integration  by  parts  and  the  definition  of  m.j^T). 

The  last  term  on  the  right  side  of  (47)  can  be  similarly  expanded. 

~  £mr),mr),U(r)]]dr 

k=  1  *=  1  j=l 

=  tE  E  («.(r)uJ(r)-'fii(T)ui(T))ujt(r)dT[Afc,[Ai,Aj]] 

4  k= l  i,i=l;»<i  ^  ,/0 

=  ji  E  m«(r)[/4,[A,/y] 

fc=l  i,j=l;t<j 

=  -tE  E  m«^(r)P*^i]>^] 

fc=l 

=  ~E(tatE  E  (50) 

P=1  k  =  l  i,j=l;i<j 

Therefore,  substituting  (48),  (49)  and  (50)  into  (47)  yields  the  expression  for  Z ^  given 
by  (33).  □. 

The  terms  of  Z^(t)  (33)  can  be  characterized  as  follows.  The  first  term  on  the 
right  side  of  (33)  is  an  0(e)  periodic  term.  The  second  term  is  a  secular  term  (linear  in 
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t)  with  an  0(e2)  periodic  term  superimposed.  The  secular  term  is  proportional  to  the 
areas  Area,j(T)  and  the  structure  constants  T?-  associated  with  the  choice  of  basis  for 
Q.  The  third  term  of  (33)  is  purely  a  secular  term  proportional  to  the  first  moments 
mijk(T )  and  the  depth-two  structure  constants  0Pjk  associated  with  choice  of  basis  for 
Q.  This  interpretation  makes  Theorem  5  an  area-moment  rule. 

It  should  be  noted  that  the  formula  of  Theorem  5  is  clearly  basis  independent. 
Additionally,  because  system  (2)  is  left-invariant,  Theorem  5  actually  gives  the  formula 
for  the  third-order  approximation  X^(t)  to  the  solution  X(t)  of  (2)  for  any  initial 
condition  X(0)  €  G.  Let  Xj(t)  and  Xj3\t )  correspond  to  the  actual  and  approximate 
solutions,  respectively,  of  (2)  with  X/(0)  =  I  €  G.  By  left-invariance  of  (2),  X(t)  = 
X(0)Xi(t)  and  X(3\t)  =  X(0)X}3)(f)  is  an  0(e3)  approximation  of  X(t)  on  an  0(  1/e) 
time  interval. 

As  discussed  earlier,  X^(t)  captures  the  effect  of  depth-two  Lie  brackets.  This  effect 
in  the  context  of  controllability  is  summarized  in  the  next  theorem. 

Theorem  6.  Suppose  that  system  (2)  satisfies  the  Lie  algebra  controllability  rank  con¬ 
dition  with  up  to  depth-two  Lie  brackets.  Then  the  complete  constructive  controllability 
problem  (P)  can  be  solved  with  0(e3)  accuracy  using  the  formula  for  X^3\t)  given  by 
(33)  and  (34). 

Proof.  Consider  a  system  of  the  form  (2)  with  nm+i(-)  =  ...  =  u„(-)  =  0,  m  <  n. 
Without  loss  of  generality  we  can  assume  that  X,  =  I  £  G  and  X/  £  Q  C  G  is  such 
that  Zj  —  'P-1(Xj)  =  0(e2).  This  is  possible  due  to  the  left-invariance  of  the  system 
and  the  fact  that  Theorem  5  can  be  applied  repeatedly.  Let 

C  =  {C  |  G  =  Ap  or  C  =  [Ai,  Aj],  or  C  =  [[A,-,  A,],  A*],  p,i,j,k  =  1, . . . ,  m}. 

By  hypothesis, 

m  77i  m 

Q  spanC  { ^  '  CpAp  -j-  ^  Aj]  -}-  ^  *]  c»j&[[Ai,  Aj],  A^],  Cp,  ct-j,  (‘ijk  £  T ] 

p=!  i,j= 1  i,j,k=l 
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wt  n  m  m  m 

=  (X  CP^P  +  XX  X  +  X  X  CiJk^jh)AP,  CP,  Cij,Cijk  £  5ft}. 

p=l  p=l  ij=l;i<i  k=li,j=l;i<j 

Therefore,  since  Zf  £  Q,  3 cp,  c;j,  c;jk  £  5ft,  p,  i,j,  k  =  1, . . .  ,m  such  that 

m  n  m  mm 

£/  =  E<vt.  +  E(  E  cij-r'i  +  E  E  (si) 

P=1  P=1  i,j-l\i<j  k=li,j=l;i<j 

Also,  from  (33)  and  the  assumption  that  «,•(•)  =  0  for  i  =  m  +  1, . . . ,  n  we  have  that 

m  n  m  mm  3r 

z's)(<,e)  =  E™P(<)^  +  E(  E  <?<*(*) rt-E  E  (52) 

P=1  p=l  fc=l  «,j=l;»'<j  •* 

So  if  we  choose  up(t),  t  £  [0, i /],  p  =  such  that 

£Up(tf )  —  Cp,  p  —  1,...,  771,  (53) 

e2ciij(tf)  =  Cij,  z,  j  =  1, . . . ,  772,  and  (54) 

-  -jrmijk(T)  =  cijki  i,j,k  =  l,...,77z,  (55) 

then  from  (51)  and  (52)  Z^)  =  This  implies  that  X^(tf)  =  $(Z<3>(i/))  = 
^(Z/)  =  X/.  So,  by  Theorem  5,  ||A(i/)  —  X/||  =  0(e3). 

It  remains  to  show  that  (53)  -  (55)  can  be  met.  This  becomes  clear  by  recognizing 
the  geometric  meaning  of  the  terms  a,j(f)  and  mijk(T).  For  q  an  integer,  a,y(gT)  = 
qArea,ij(T)  and  Area^iT )  is  the  area  bounded  by  the  closed  curve  described  by  u,-  and 
uj  over  one  period.  In  particular,  if  we  choose  ft,-  and  Uj  to  be  sinusoids  that  are  in  phase 
then  Area{j(T)  =  0.  Alternatively,  if  they  are  chosen  out  of  phase  then  Area,ij(T)  ^  0 
is  a  function  of  the  product  of  the  signal  magnitudes  and  phase  difference.  The  terms 
mijk(T)  are  first  moments  as  described  above.  In  particular,  if  we  choose  ut-  and  Uj  to 
be  sinusoids  that  are  in  phase  then  m,jfc(T)  =  0.  Only  when  ft,-  and  Uj  are  nonzero 
and  out  of  phase  and  uk  is  also  nonzero  will  m,jfc  ^  0.  In  this  case  mijk(T )  will  be 
proportional  to  the  product  of  the  signal  magnitudes.  Based  on  this  reasoning  the  final 
values  of  each  of  these  terms  can  be  matched  independently,  i.e.,  (53)  -  (55)  can  be  met. 
The  timing  can  be  controlled  by  choosing  the  frequency  and  amplitudes  of  the  sinusoids 
appropriately.  □ 
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4  Underwater  Vehicle  Control 


Consider  an  autonomous  underwater  vehicle  and  let  (ri,r2,r3)  be  coordinates  fixed  on 
the  vehicle.  Let  (bi,b2,b3)  be  inertial  coordinates.  Then  we  define  X(t)  €  SE( 3) 
where 

T  I  m(+\  1 

,  XR(t)  e  50(3),  xT(t)  €  5ft3, 


m  = 


’  xR(t) 

xT(t) 

0  0  0 

1 

such  that 

X(t) 


ri 

i _ i 

xT(t) 

1 

000 

1 

r; 

1 


XR{t)r  i  +  xT(t) 

1 


bi 

1 


That  is,  X(t )  describes  the  orientation  and  position  of  the  vehicle  at  time  t.  Let  ex  = 
(1,0, 0)T,  e2  =  (0,1, 0)r  and  e3  =  (0, 0,  l)r. '  Define  A :  5ft3  ->  so(3)  where  so( 3)  is  the 
space  of  skew  symmetric  matrices  and  x  =  (xi,  x2,  x3)r,  by 


x  — 


0  — x3  x2 

x3  0  — Xi 

— x2  xx  0 


Let 


A,= 


e,- 

0 

0  0  0 

0 

0 

e,-_ 

3 

0  0  0 

0 

i  =  1,2,3 
*  =  4,5,6. 


Then  {Ai, . . . ,  A6}  defines  a  basis  for  Q  =  ae( 3),  the  Lie  algebra  associated  with  525(3). 
Now  let  O  =  (fii,  ft2,  ^3)T  define  the  angular  velocity  of  the  vehicle  and  v  =  (i>i,  v2,  v3)T 
the  vehicle  translational  velocity  all  with  respect  to  (ri,r2,r3).  Then  X(t )  satisfies 


3  6 

X  =  +  I>i(*Mi). 

J=4 


(56) 


1=1 


Based  on  the  discussion  m  the  introduction,  we  assume  that  we  can  interpret  fi(t) 
and  v(t)  as  controls  such  that  (56)  is  of  the  form  (2).  Specifically,  suppose  that  we 
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have  four  controls  available,  i.e.,  eu,(t)  =  fl,(t),  i  =  1,2,3  and  eu4(t)  -  vi(t).  Let 
Fi(X )  =  XA,-,  i  =  1, . . .  ,4,  then 

X  =  eX^AiUi)  =  e'£n{X)ui.  (57) 

i=i  i=i 

Now,  the  Lie  bracket  of  left-invariant  vector  fields  on  a  matrix  Lie  group  can  be  ex¬ 
pressed  in  terms  of  the  Lie  bracket  on  the  associated  Lie  algebra  as  [F,(X),  Fj(X)]  = 
[XA{:XAj]  =  X[Ai,Aj].  Thus,  since  [A3,  A4]  —  A5  and  [A4,A2]  =  A6,  the  system  is 
completely  controllable  with  the  Lie  algebra  rank  condition  requiring  only  depth-one  Lie 
brackets.  However,  if  only  three  controls  were  available,  e.g.,  if  one  of  the  controls  failed, 
then  the  controllability  situation  changes.  Certainly,  if  the  translational  control  u4  is 
lost  then  position  control  is  lost.  However,  if  one  of  the  rotational  controls  is  lost  the 
system  is  still  controllable.  Specifically,  if  u4  is  lost  then  the  system  still  satisfies  the  Lie 
algebra  rank  condition  with  depth-one  brackets.  On  the  other  hand,  if  u2  or  u3  is  lost, 
depth-two  Lie  brackets  are  needed  to  satisfy  the  Lie  algebra  rank  condition.  For  exam¬ 
ple,  suppose  ui,  u2  and  u4  are  the  controls  available.  Then  [Aj,  A2]  =  A3,  [A4,  A2]  =  A6 
and  [[Ai,  A2],  A4]  =  A5  show  the  system  is  controllable  using  one  depth-two  Lie  bracket. 
Thus,  in  the  general  case  where  one  translational  and  two  angular  controls  are  available, 
the  third-order  average  formula  (and  not  the  second-order  average  formula)  provides  a 
means  to  derive  controls  for  complete  control  of  the  vehicle. 

In  effect,  one  can  think  of  the  third-order  average  formula  as  providing  an  “adaptive” 
control  law  for  translating  and  orienting  an  autonomous  underwater  vehicle.  Specifically, 
under  normal  conditions  with  four  controls  available,  the  control  algorithm  could  be 
based  on  the  second-order  average  formula.  In  the  event  of  a  failure  that  reduces  control 
authority  to  three  controls,  the  control  algorithm  could  be  switched  to  one  based  on  the 
third-order  average  formula.  In  this  scenario  the  controller  would  adapt  to  the  failure 
and  continue  to  effect  complete  control  over  the  vehicle’s  position  and  orientation. 

We  now  illustrate  how  to  use  the  third-order  average  formula  (i.e.,  the  area-moment 
rule)  to  construct  controls  to  solve  problem  (P),  i.e.,  to  translate  and  orient  an  underwa¬ 
ter  vehicle  as  desired,  in  the  general  case  when  one  translational  and  two  angular  controls 
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are  available.  Specifically,  we  focus  on  system  (57)  where  u3(-)  =  0,  i.e.,  u1?  u2  and  u4 
are  the  controls  available,  as  in  the  example  above.  From  Theorem  5  we  can  write  down 
the  formula  for  the  third-order  approximation  X^3\t)  to  the  solution  X(t)  €  SE( 3)  as 

x<3>«  =  z(s>(0  =  £4.(04.. 

p=l 


di(t)  =  eu^t)  -  ym2i2(T),  d4(t)  =  eu4(t)  -  ^m242(T), 

d2(t)  =  tu2{t)  -  ~m121(T),  ds(t)  =  -^(m124(T)  +  m421(T)),  (58) 

J3(0  =  e :2a42(t ),  J6(t)  =  e2a42(i). 

Now  suppose  without  loss  of  generality  that  X(0)  =  I  and  it  is  desired  that  X(tj)  =  Xd 
such  that  Zi  =  ^_1(X(i)  =  0(e2)  (see  the  proof  of  Theorem  6).  Let  Xrj  an(i  xTd  be  the 
corresponding  desired  rotational  and  translational  parts  of  Xd .  To  solve  the  problem 
(P)  with  <3(e3)  accuracy,  we  derive  an  algorithm  such  that  X^(tf)  =  Xd  and  apply 
Theorem  5. 


To  simplify  our  task,  we  solve  the  translational  part  of  the  problem  first  and  then 
the  rotational  part.  Recall  that  X(t)  =  exp where  we  can  express  Z(t)  = 
£p=1  dp(t)Ap.  Similarly,  Xd  —  exp {Zj)  where  we  can  express  Zd  —  Y?p=\ddvAp-  Thus, 
the  translational  part  of  the  problem  is  to  specify  controls  such  that  dp(tf)  =.ddp,  p  = 
4,5,6,  and  the  rotational  part  of  the  problem  is  to  specify  controls  such  that  dp(tf)  = 
ddv ,  p  —  1,2,3.  It  can  be  seen  from  (58)  that  we  need  the  third-order  (0(e3))  averaging 
formula  of  Theorem  5  to  solve  the  translational  part  of  the  problem  (otherwise,  d5(t) 
would  be  identically  zero).  However,  we  only  need  the  second-order  (0(e2))  averaging 
formula  to  solve  the  rotational  part  of  the  problem  where  we  take  e  =  e3/2.  In  fact,  for 
the  rotational  problem  we  can  use  an  algorithm  similar  to  the  one  used  for  the  attitude 
control  problem  of  [6]  which  uses  Theorem  2  and  the  Wei-Norman  parametrization. 
This  is  because  of  our  choice  of  basis  (Ai, . . . ,  An).  The  Wei-Norman  equations  for 
G  =  SE( 3)  with  our  chosen  basis  are 
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and  it  can  be  seen  that  the  parameters  771,  g2,  <73,  which  parametrize  the  orientation 
of  the  vehicle,  correspond  to  the  three  parameters  in  the  attitude  control  problem. 
The  parameters  <74,  775,  g6  parametrize  the  position  of  the  vehicle,  and  it  is  easy  to 
show  that  di  =  <7,-,  i  =  4,5,6.  So  the  desired  orientation  Xnd  is  expressed  in  Wei- 
Norman  parameters  {9d\-,9d2,9ds)  and  the  desired  position  XTd  is  expressed  identically 
in  Wei-Norman  and  single-exponential  parameters  (77^, 77^5,  9do)  =  (dd4,  dd5,  dd6).  It  is 
additionally  assumed  that  it  is  desired  that  u,-,  i  =  1,2,4  be  continuous  and  u,(0)  = 
U{(tf)  -  0,  i  =  1,2,4. 


The  following  algorithm  uses  sinusoidal  controls  and  has  been  derived  according  to 
the  geometric  reasoning  outlined  in  the  proof  of  Theorem  6  (for  the  translational  part  of 
the  problem)  and  the  proof  of  Theorem  4  [6]  (for  the  rotational  part  of  the  problem).  The 
time  interval  [0,tf]  is  divided  into  subintervals  [U,tj]  such  that  tx  =  ^  =  j,  t2  =  h  +  qT, 

tz  —  ^2  +  rT,  t4  =  ^3  + A4T ,  ^5  =  £4  +  te  —  ^5  +  ^7  —  ^6  +  "Jj  ^8  =  ^7  +  sT,  tg  = 

iio  =  tj  =  ig  +  j  where  q,r,  s  and  A4  are  described  below.  The  algorithm  defines  the 
controls  as  follows: 


eui(f) 


0  0  <t<h 

—a,\U)sin(u{t  —  tx))  tx  <  t  <  t2 
0  t2  <  t  <t7 


—a4fjjsin{u}{t  —  £7)) 


c«i(<)  =  <  0 


t?  <£.  t  <  t& 
tg  <  t  <  to 


| gdlusin(uj(t  -  t9))  t9  <  t  <  tw 
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eu2(t)  =  < 


0 

a2uisin{uj{t  —  t4 )) 
a3usin(u(t  —  i2)) 


0  <t<h 
ti  <  t  <  t2 
t2  <.  t  <  t3 


l  0 


^3  <  t  <  te 


tu2{t)  =  { 


gd2usin(u(t  -  t6)) 
-\gd2wsin(u{t  - t9)) 


t6<t 
t9  <t 


eu4(t)  =  < 


dd4usin(u>t) 

(dd4ucos(ujA4T))cos(u)(t  —  t4 )) 

—  i(<^4u;cos(a;A4T))sm(u>(2  —  ^5)) 


l  0 


Positive  integers  q,r  and  s  and  ai,a2,a3,a4  £  3?  are  selected 
rules: 


<  <9 
5;  ^10 

0  <t<t5 
t4  <  t  <t5 
t$  <  t  <  t9 
te  <  t  <  t\ 0 

according  to  the  following 


dds 

axa2q  = 

dd  47T 

(60) 

dd6 

«3  r  =  ,  a2q. 

dd4x 

(61) 

a\q  +  a^r  =  8  <  — . 

7 r 

(62) 

9d3 

O4S  — 

(63) 

7!" gd2 

Then  A4,  the  period  of  oscillation  T  and  the  frequency  u  can  be  computed  as 


A4  — 
T  — 


sin  1{— ^(a^g  +  a^r)} 
2tt 


5 


_ tl _ 

{g  +  t  +  s  ■+■  A4  +  2)  ’ 


UJ  = 


2i r 

~T' 


As  an  example,  we  use  this  algorithm  to  translate  and  orient  an  autonomous  under¬ 
water  vehicle  to  a  desired  position  corresponding  to  dd4  —  0.2,  —  0.05  and  dde  —  0.2 

and  a  desired  orientation  corresponding  to  gdl  =  0.05  radians,  gd2  =  0.05  radians  and 
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Figure  1:  Vehicle  in  (a)  Initial  and  (b)  Desired  Position  and  Orientation. 

gd3  =  0.04  radians  in  tf  =  29.2  units  of  time.  Figure  1(a)  shows  the  vehicle  in  its  initial 
position  and  orientation  (at  the  identity  of  SE( 3)),  and  Figure  1(b)  shows  the  vehicle 
in  its  desired  position  and  orientation.  In  each  of  these  figures,  the  vehicle  is  shown 
together  with  its  projected  image  on  the  x-z  plane  (at  y  —  0.7)  and  on  the  y-z  plane 
(at  x  =  0.7).  The  *  symbols  indicate  the  desired  position  of  the  center  of  the  vehicle 
projected  onto  the  x-z  and  y-z  planes. 

This  example  is  intended  to  represent  one  step  in  a  multi-step  maneuver.  As  dis¬ 
cussed  earlier  feedback  could  be  used  between  steps  to  improve  accuracy.  We  also  note 
that  in  using  the  algorithm  above,  there  is  a  great  deal  of  flexibility  in  choosing  the 
constants  according  to  the  rules  (60)  -  (63).  In  this  example,  the  constants  were  chosen 
to  keep  the  frequency  of  the  control  signals,  u,  relatively  low.  In  particular,  we  used 
q  =  4,  r  =  4,  s  =  4,  fli  «  0.11,  a 2  ~  0.18,  a3  «  —0.10,  and  04  ~  0.06  such  that 
A4  =  7/12,  T  =  2  and  u  =  7T.  Additionally,  e  =  0.2  and  e  =  e3/2  ~  0.09. 

Figure  2  shows  the  three  controls  iq,  u2  amd  U4,  as  a  function  of  time.  Figure  3  shows 
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Figure  2:  Control  Input  Signals  for  Example. 

a  simulation  of  the  response  of  the  system  (solid  lines).  The  simulation  was  produced  by 
numerically  solving  the  Wei-Norman  equations  (59)  using  MATLABR.  The  orientation 
of  the  vehicle  is  given  in  Figures  3(a),  3(b)  and  3(c)  which  show  plots  of  gi,  g2,  gz, 
respectively.  The  position  of  the  vehicle  is  given  in  Figures  3(d),  3(e)  and  3(f)  which 
show  plots  of  c/4,  c/5,  c/g,  respectively.  The  dashed  lines  represent  the  corresponding 
average  values  of  the  parameters  as  a  function  of  time  computed  directly  from  the 
average  formula.  It  is  clear,  by  comparing  the  solid  lines  to  the  dashed  lines  in  Figure 
3,  that  at  the  end  of  the  simulation  ||X(f/)  —  Xd\\  =  0(e3),  i.e.,  the  vehicle  has  been 
moved  and  oriented  as  desired  with  0(e3)  accuracy. 
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